#include "AssociationSet.h"

AssociationSet::AssociationSet(){
	//ROS_INFO("create AssociationSet");
}
AssociationSet::AssociationSet(FrameNode * src,FrameNode * dst){
	int association_id_counter = 0;
	vector<Plane *> src_planes = src->frame->planes;
	vector<Plane *> dst_planes = dst->frame->planes;
	for(unsigned int i = 0; i < src_planes.size() ; i++){
		for(unsigned int j = 0; j < dst_planes.size() ; j++){
			PlaneAssociation * p_a = new PlaneAssociation(src_planes.at(i),dst_planes.at(j));
			p_a->id = association_id_counter;
			association_id_counter++;
			possible_associations.push_back(p_a);
		}
	}
	
	val = min(src_planes.size(),dst_planes.size());
	len = association_id_counter;
}

AssociationSet::~AssociationSet(){}

char * AssociationSet::getHashValue(){
	char * hash = new char [1+len/8];
	for(int i = 0; i < 1+len/8; i++){hash[i] = 0;}
	for(unsigned int i = 0; i < selected_associations.size(); i++)
	{
		int current_id = selected_associations.at(i)->id; 
		hash[current_id / 8] += 1 << (current_id % 8);
	}
	return hash;
};

AssociationSet AssociationSet::clone(){

	AssociationSet ret;
	ret.val = val;
	ret.len = len;
	for(unsigned int i = 0; i < selected_associations.size(); i++)
	{
		ret.selected_associations.push_back(selected_associations.at(i));
	}
	for(unsigned int i = 0; i < possible_associations.size(); i++)
	{
		ret.possible_associations.push_back(possible_associations.at(i));
	}
	return ret;
};

void AssociationSet::select_association(Association * t){
	//ROS_INFO("select_association: %i",t->id);
	
	if(t->type == 0)
	{
		PlaneAssociation * test = (PlaneAssociation *)t;
		for(unsigned int i = 0; i < possible_associations.size(); i++)
		{
			Association * tmp_a = possible_associations.at(i); 
			if(tmp_a->type == 0)
			{
				PlaneAssociation * tmp_pa = (PlaneAssociation * )tmp_a;
				//ROS_INFO("%i == %i || %i == %i", tmp_pa->p1->id,test->p1->id,tmp_pa->p2->id,test->p2->id);
				if((tmp_pa->p1 == test->p1 || tmp_pa->p2 == test->p2) || !testAssociation(t))
				{
					//ROS_INFO("association %i is no longer an option",tmp_pa->id);
					Association * tmp_as = possible_associations.at(i);
					possible_associations.at(i) = possible_associations.at(possible_associations.size()-1);
					possible_associations.at(possible_associations.size()-1) = tmp_as;
					
					possible_associations.pop_back();
					i--;
				}
			}
		}
	}
	//ROS_INFO("done select_association: %i",t->id);
	selected_associations.push_back(t);
};

bool AssociationSet::testAssociation(Association * t){
	//ROS_INFO("++++++++++++++++++++++testAssociation++++++++++++++++++++++");
	if(t->type == 0)
	{
		PlaneAssociation * test = (PlaneAssociation * )t;

		float src_n_x 		= test->p1->normal_x;
		float src_n_y 		= test->p1->normal_y;
		float src_n_z 		= test->p1->normal_z;
	
		float dst_n_x 		= test->p2->normal_x;
		float dst_n_y 		= test->p2->normal_y;
		float dst_n_z 		= test->p2->normal_z;
		for(unsigned int i = 0; i < selected_associations.size(); i++)
		{
			Association * tmp_a = selected_associations.at(i); 
			if(tmp_a->type == 0)
			{
				PlaneAssociation * tmp_pa = (PlaneAssociation * )tmp_a;

				float src_angle = tmp_pa->p1->normal_x*src_n_x + tmp_pa->p1->normal_y*src_n_y + tmp_pa->p1->normal_z*src_n_z;
				float dst_angle = tmp_pa->p2->normal_x*dst_n_x + tmp_pa->p2->normal_y*dst_n_y + tmp_pa->p2->normal_z*dst_n_z;
				float angle_diff = fabs(fabs(src_angle)-fabs(dst_angle));
				//ROS_INFO("assosi %i <-> %i",t->id,tmp_a->id);
				//ROS_INFO("src_angle:  %f",src_angle);
				//ROS_INFO("dst_angle:  %f",dst_angle);
				//ROS_INFO("angle_diff: %f",angle_diff);
				if(angle_diff > 0.1){return false;}
			}
		}
	}else{
		return false;
	}
};

std::vector<AssociationSet> AssociationSet::explore(){
	//ROS_INFO("+++++++++++++++explore+++++++++++++++");
	std::vector<AssociationSet> asoSet;
	for(unsigned int i = 0; i < possible_associations.size(); i++)
	{
		AssociationSet tmp = clone();
		tmp.select_association(possible_associations.at(i));
		asoSet.push_back(tmp);
	}
	//ROS_INFO("---------------explore---------------");
	return asoSet;
};

double AssociationSet::estimateSearchValue(){
	return val;
};

